/*
 * Vector2.cpp
 *
 *  Created on: 2011/06/22
 *      Author: naoki
 */

#include "NTEngine/NTEngine.h"
#include "NTEngine/Math/Function.h"
#include "NTEngine/Math/Vector2.h"

namespace NTEngine {
namespace Math {

Vector2::Vector2( float aX, float aY )
: x( aX ), y( aY ) {
}

Vector2::Vector2( int aX, int aY )
: x( static_cast< float >( aX ) ), y( static_cast< float >( aY ) ) {
}

Vector2::Vector2( const Vector2& a )
: x( a.x ), y( a.y ) {
}

void Vector2::operator=( const Vector2& a ) {
	x = a.x;
	y = a.y;
}

void Vector2::operator+=( const Vector2& a ) {
	x += a.x;
	y += a.y;
}

void Vector2::operator-=( const Vector2& a ) {
	x -= a.x;
	y -= a.y;
}

void Vector2::operator*=( float a ) {
	x *= a;
	y *= a;
}

void Vector2::operator/=( float a ) {
	float t = 1.F / a;
	x *= t;
	x *= t;
}

bool Vector2::operator==( const Vector2& a ) const {
	return (x == a.x) && (y == a.y);
}

bool Vector2::operator !=( const Vector2& a ) const {
	return (x != a.x) || (y != a.y);
}

void Vector2::set( float ax, float ay ) {
	x = ax;
	y = ay;
}

void Vector2::setAdd( const Vector2& a, const Vector2& b ) {
	x = a.x + b.x;
	y = a.y + b.y;
}

void Vector2::setSub( const Vector2& a, const Vector2& b ) {
	x = a.x - b.x;
	y = a.y - b.y;
}

float Vector2::length() const {
	return sqrt( ( x * x ) + ( y * y ) );
}

void Vector2::zero() {
	x = y = 0.F;
}

void Vector2::hermite(
	const Vector2& p0, const Vector2& v0, // 始点
	const Vector2& p1, const Vector2& v1, // 終点
	float t, Vector2* out
) {
// 3時曲線
// f(u)=au^3+bu^2+cu+dを考える
// 始点 p0 = f(0) = d …①
// 終点 p1 = f(1) = a + b + c + d …②
// 次に各点での法線、これは微分でおｋ
// f’(u)=3au^2+2bu+c
// 始点の法線 v0 = f’(0) = c…③
// 終点の法泉 v1 = f’(1) = 3a + 2b + c…④
// ①～④から
// p1 = a + b + v0 + p0
// v1 = 3a + 2b + v0
// a,bについて解くと
// a = 2p0 + v0 - 2p1 + v1
// b = -3p0 + -2v0 + 3p1 - v1
// で、次の式がでる
// f(u) = (2p0 + v1 - 2p1 + v1)u^3 + (-3p0 + -2v0 + 3p1 - v1)u^2 + (v0)u + p0
	ASSERT((t >= 0.0F) && (t <= 1.0F));

	float t2 = t * t; // u^2
	float t3 = t2 * t; // u^3
	float cP0 = 2.F * t3 - 3.F * t2 + 1.F; // p0について Coefficient
	float cV0 = t3 - 2.F * t2 + t; // v0について
	float cP1 = -2.F * t3 + 3.F * t2; // p1
	float cV1 = t3 - t2; // v1

	float x = p0.x * cP0 + v0.x * cV0 + p1.x * cP1 + v1.x * cV1;
	out->y = p0.x * cP0 + v0.y * cV0 + p1.y * cP1 + v1.y * cV1;
	out->x = x;
}

void Vector2::bezier(
	const Vector2& p0,
	const Vector2& p1, const Vector2& p2, // 制御点
	const Vector2& p3,
	float t, Vector2* out)
{
// 始点と制御点1の内分点A pa = (1 - u)p0 + (u)p1
// 終点と制御点1の内分点B pb = (1 - u)p1 + (u)p3
// ABの内分点C pc = (1 - u)pa + (u)pb
// pc = (1 - u)^2p0 + 2(u)(1 - u)p1 + (u)^2p3
// 始点と制御点2の内分点D pd = (1 - u)p0 + (u)p2
// 終点と制御点2の内分点E pe = (1 - u)p2 + (u)p3
// DEの内分点F pf = (1 - u)pd + (u)pe
// pf = (1 - u)^2p0 + 2(u)(1 - u)p2 + (u)^2p3
// CFの内分点G pg = (1 - u)pc + (u)pf
// pg = (1 - u)^3p0 + 3(u)(1 -u)^2p1 + 3(u)^2(1-u)p2 + (u)^3p3
	ASSERT((t >= 0.0F) && (t <= 1.0F));

	float t2 = t * t; // u^2
	float s = 1.F - t;
	float s2 = s * s; // (1 - u)^2
	float cP0 = s2 * s;
	float cP1 = 3.F * t * s2;
	float cP2 = 3.F * t2 * s;
	float cP3 = t2 * t;

	float x = p0.x * cP0 + p1.x * cP1 + p2.x * cP2 + p3.x * cP3;
	out->y = p0.x * cP0 + p1.y * cP1 + p2.y * cP2 + p3.y * cP3;
	out->x = x;
}

} // namespace Math
} // namespace NTEngine
